//Erik Wright
//marionette.cpp
//11-4-2013

/*
This file describes a robotic marionette puppet.  The goal of this file is to demonstrate 
a genetic algorithm that allows the puppet to 'learn' how to move.  This movement could be walking,
running, jumping, or really anything that comes to mind.  This particular algorithm will 
eventually be used to define the movements of the puppet as it dances.  Since there has already
been a lot of care going into the puppet, this functional description has been written to be as
fully descriptive as possible.

For more documentation on the motor controller for the puppet, see the headding of the bot_motion.h
file.
*/

#include "humanoid_marionette.h"
#include <iostream>
using namespace std;



//---------------------------------------humanoid_marionette-------------------------------------
//__________________private functions

//correct_slack()
/*
This function corrects the slack line 
*/
void humanoid_marionette::correct_slack(int channel_a, int channel_b)
{

}



void humanoid_marionette::correct_stiff(int channel_a, int channel_b)
{

}





//__________________public functions

//~humanoid_marionette
/*
This function is the class deconstructor.  It will completely reallocate 
any initialized memory back on to the stack.
INPUT:	none
OUTPTU:	deconstruction
*/
humanoid_marionette::~humanoid_marionette()
{
	for(int i=0; i<NUM_CHANNELS; i++)
	{
		delete controller_array[i];
	}	
	
	return;
}



//humanoid_marionette(int)
/*
this is the default class constructor.  It will create an array of controller channels 
of length int channels and will initialize a list of events being int frames long.  
This is particularly useful when making an initial population of random sequences to 
make a genetic algorithm out of.
INPUT:	none
OUTPUT:	initialization
*/
humanoid_marionette::humanoid_marionette(int frames)
{
	motion_event* toins;

	for(int h=0; h<NUM_CHANNELS; h++)		//initialize every channel with at least the all stop command 
	{
		toins = new motion_event(1500, 10);	//initialize all stop value
		controller_array[h].insert_at_end(toins);	

		for(int i=0; i<frames; i++)
		{
			toins = new motion_event();
			controller_array[h].insert_at_end(toins);	
		}
		toins = new motion_event(1500, 10);		//create an all-stop command
		controller_array[h].insert_at_end(toins);	//end with an all-stop command
	}

	//later I will add the error check function
	//this way newly created classes will not have
	//error
}




//humanoid_marionette()
/*
This is the default copy constructor.
This function will create a copy of the given class.
INPUT:	humanoid_marionette*&	|class instance to copy
OUTPUT:	void
*/
humanoid_marionette::humanoid_marionette(const humanoid_marionette*& given)
{

}




//humanoid_marionette()
/*
This function takes the name of a file in the current 
working directory and uses it to initialize a class 
instance.  This is useful for recalling specific 
motions that were created earlier.
INPUT:	char*	| name of the file
OTUPUT:	void
*/
humanoid_marionette::humanoid_marionette(char* fname)
{

}




//error_check()
/*
This function will search through the controller
array and search for errors between controllers.
An error is defined by a connection point that 
is raised above or lowered below the critical
length of the rigid structure between the two.
INPUT:	none
OUTPUT:	bool	|true if error exists
*/
bool humanoid_marionette::error_check()
{
	
}



//play_motion()
/*
This function will play the current chromosome 
on the marionette puppet.  You must first set
up a serial connection to the puppet's controller.
INPUT:	int	|file descriptor for channel
OUTPUT:	int	|negative if error	
*/
int humanoid_marionette:play_motion(int file_des)
{
	
}



//to_file()
/*
This function will write the entire class
out to a file.  This is extremely handy if
you would like to save sequences and chromosomes
INPUT:	char*	|Name of the file
OUTPUT:	void	
*/
void humanoid_marionette::to_file(char* fname)
{
	FILE* fd = fopen(fname,"w");

	for(int i=0; i<NUM_CHANNELS; i++)
	{
		controller_array[i].printout(fd);
	}

	close(fd);
}





//display_sequence()
/*
this function provides standard output display for the current class instance.
This could be thought of a chromosome in a genetic algorithm.
INPUT:	none
OUTPUT:	standard display
*/
void humanoid_marionette::display_sequence()
{
	for(int i=0; i<NUM_CHANNELS; i++)	//for every element in the array
	{
		cout<<"Channel"<<i<<":\t";
		controller_array[i].inorder_display();

	}
}



//display_errors()
/*
this function displays the error matrix.  Meaning
any detected errors after calling error_check()
will be displayed.  If a change has been issued
to the error matrix after the chromosome has been
changed in any way, the display will not be correct.
INPUT:	void
OUTPUT:	standard out
*/
void humanoid_marionette::display_errors()
{
	cout<<"Currently Known Errors..."<<endl;	//print title
	cout<<"   \t";
	for(int j=0; j<NUM_CHANNELS; j++)	//print horizontal channels numbers
	{
		cout<<"Ch: "<<j<<"\t";
	}
	cout<<endl;
	for(int h=0; h<NUM_CHANNELS; h++)
	{
		cout<<"Ch: "<<h<<"\t";
		for(int i=0; i<NUM_CHANNELS; i++)
		{
			cout<<error[h][j]<<"\t";
		}
		cout<<endl;
	}

	cout<<endl<<endl;
}



//display_current_disp()
/*
This function will display the current displacement
of each controller as it is reported by each controller.
INPUT:	none
OUTPUT:	standard out
*/
void humanoid_marionette::display_current_disp()
{
	cout<<"Current Displacements..."<<endl;
	
	for(int i=0; i<NUM_CHANNELS; i++)
	{
		cout<<"channel: "<<i<<": ";
		controller_array[i].display_disp();
		cout<<endl;
	}

	cout<<endl<<endl;
}



//display_current_velo()
/*
This function will display the current velocity
of each controller as it is reported by each controller.
INPUT:	none
OUTPUT:	standard out
*/
void humanoid_marionette::display_current_velo()
{
	cout<<"Current velocities..."<<endl;
	
	for(int i=0; i<NUM_CHANNELS; i++)
	{
		cout<<"channel: "<<i<<": ";
		controller_array[i].display_velo();
		cout<<endl;
	}

	cout<<endl<<endl;
}



//give_rating()
/*
This function will set the rating value of the 
current chromosome.  This is a necesity when
implementing a genetic algorithm to evolve 
generic motions in the robot.
INPUT:	int	|value between 0 and 100
OUTPUT:	bool	|true if successful rating	
*/
bool humanoid_marionette::give_rating(int rate)
{
	if((rate<0)||(rate>100))
	{
		cout<<"Please enter a valid rating.  (0->100)"<<endl;
		return;
	}
	else
	{
		rating = rate;
		return;
	}
}



//binary_twist()
/*
This function will perform a binary twist
between the given chromosome and the 
calling cromosome.  This function will immediately
call the error checking method.
INPUT:	humanoid_marionette*&	|to be twisted with
OUTPUT:	void
*/
void humanoid_marionette::binary_twist(humanoid_marionette*& given)
{

}



//binary_twist_rand()
/*
This function will perform a binary twist at 
random points in the chromosome between the given
class instance and the calling class instance.
There is an option to preserve the length of the 
chromosome; however, this is experimental and should
be played with.
INPUT:	humanoid_marionette*&	|to be twisted with
	bool	|whether or not to preserve list lengths
OUTPUT:	void
*/
void humanoid_marionette::binary_twist_rand(humanoid_marionette*& given, bool preserve)
{
	
}



//average_twist()
/*
This functionw ill perform a twist as the average
of the two chromosomes involved with the twist.
Only the calling function will be changed, and 
the given function will be preserved.
INPUT:	humanoid_marionette*&	|to be twisted with
OUTPUT:	void
*/
void humanoid_marionette::average_twist(humanoid_marionette*& given)
{

}



//weighted_average_twist()
/*
This function will perform a weighted average 
between the calling and given class instances.
Only the calling function will be changed, and 
the given function will be preserved.  The input
signifies the wieght to be attributed to the 
average.
INPUT:	humanoid_marionette*&	|to be twisted with
	int	|weight of the average
OUTPUT:	void
*/
void humanoid_marionette::weighted_average_twist(humanoid_marionette*& given, int weight)
{

}



//fuzzy_twist()
/*
This function will perform a fuzzy twist.
Based on the given rating of the chromosome 
and the given chromosome, a fuzzy membership
identifier will be computed, and a corresponding
twist operator will be chosen to hopefully 
remove noise in the evolution, and promote 
the continuation of desireable traights.
INPUT:	humanoid_marionette*&	|to be twisted with
OUTPUT:	void
*/
void humanoid_marionette::fuzzy_twist(humanoid_marionette*& given)
{

}
